#include "inirobmodule.hh"
#include <unistd.h>
#include "config.h"

using namespace std;

#define _PRINT(FMT, ...)  //rcs_print

// Util::IniParser ini_file(filepath);
// ini_file.get("TASK.AUTO_ENABLE", (bool&)autoenable, false);
// ini_file.get("TASK.AUTO_PROGRAM", (bool&)autoprogram, false);

std::string deterRobotModel(int& robotModel)
{
    std::string modelIniPath = ".ZU710.ini";
    switch (robotModel)
    {
    case MODEL_ZU_310:
        modelIniPath = ".ZU310.ini";
        break;
    case MODEL_ZU_311:
    case MODEL_ZU_312:
    case MODEL_ZU_313:
        modelIniPath = ".ZU311.ini";
        break;
    case MODEL_ZU_710:
        modelIniPath = ".ZU710.ini";
        break;
    case MODEL_ZU_720:
        modelIniPath = ".ZU720.ini";
        break;
    case MODEL_ZU_721:
        modelIniPath = ".ZU721.ini";
        break;
    case MODEL_ZU_722:
        modelIniPath = ".ZU722.ini";
        break;
    case MODEL_ZU_723:
        modelIniPath = ".ZU723.ini";
        break;
    case MODEL_ZU_724:
        modelIniPath = ".ZU724.ini";
        break;
    case MODEL_ZU_725:
    case MODEL_ZU_728:
    case MODEL_PRO_731:
        modelIniPath = ".ZU725.ini";
        break;
    case MODEL_ZU_512:
    case MODEL_ZU_513:
    case MODEL_ZU_514:
    case MODEL_PRO_515:
    case MODEL_ZU_726:
        modelIniPath = ".ZU726.ini";
        break;
    case MODEL_ZU_727:
        modelIniPath = ".ZU727.ini";
        break;
    case MODEL_ZU_1210:
    case MODEL_ZU_1211:
    case MODEL_ZU_1212:
    case MODEL_PRO_1213:
        modelIniPath = ".ZU1210.ini";
        break;
    case MODEL_ZU_1810:
    case MODEL_ZU_1811:
    case MODEL_ZU_1812:
    case MODEL_PRO_1813:
        modelIniPath = ".ZU1810.ini";
        break;
    case MODEL_C_510:
        modelIniPath = ".C510.ini";
        break;
    case MODEL_C_511:
        modelIniPath = ".C511.ini";
        break;
    case MODEL_C_710:
        modelIniPath = ".C710.ini";
        break;
    case MODEL_C_711:
        modelIniPath = ".C711.ini";
        break;
    case MODEL_ZU_2010:
    case MODEL_ZU_2011:
    case MODEL_ZU_2012:
    case MODEL_PRO_1610:
        modelIniPath = ".ZU2010.ini";
        break;
    case MODEL_CO_0115:
        modelIniPath = ".CO0115.ini";
        break;
    case MODEL_CO_1115:
        modelIniPath = ".CO1115.ini";
        break;
    case MODEL_CO_1315:
        modelIniPath = ".CO1315.ini";
        break;
    case MODEL_CO_1415:
        modelIniPath = ".CO1415.ini";
        break;
    case MODEL_CO_2015:
        modelIniPath = ".CO2015.ini";
        break;
    case MODEL_CO_2120:
        modelIniPath = ".CO2120.ini";
        break;
    case MODEL_AT_0710:
        modelIniPath = ".AT710.ini";
        break;
    case MODEL_MAX_3510:
        modelIniPath = ".MAX3510.ini";
        break;
    case MODEL_DU_2001_L:
    case MODEL_DU_311_L:
        modelIniPath = ".DU310.ini";
        break;
    case MODEL_DU_2002_R:
    case MODEL_DU_312_R:
        modelIniPath = ".DU311.ini";
        break;
    default:
        robotModel = MODEL_UNSUPPORTED;
        modelIniPath = ".ZU710.ini";
        _PRINT("iniLoadRobotModelData failed\n");
        break;
    }
    string ret = ZUC_USER_CONFIG_DIR "/../default/" + modelIniPath;
    return ret;
}

static void readMotionLimit(Util::IniParser& ini_file, long serialNum, RobotModelData* modelData)
{
    ini_file.get("PAYLOAD.PAYLOAD", modelData->payload, 7.0, 0.0, 50.0);  //默认使用ZU710
    //trajInifile.Find(&modelData->payload, "PAYLOAD", "PAYLOAD");
    // modelData->maxTrajVel_ori = 0.0;
    // modelData->maxTrajAcc_ori = 0.0;
    ini_file.get("TRAJ.TRAJ_MAX_VEL", modelData->maxTrajVel, 1800.0, 0.0, 7000.0);        //默认使用ZU710,最大值取zu722
    ini_file.get("TRAJ.TRAJ_MAX_ACC", modelData->maxTrajAcc, 3500.0, 0.0, 20000.0);       //默认使用ZU710,最大值取zu722
    ini_file.get("TRAJ.TRAJ_MAX_VEL_ORI", modelData->maxTrajVel_ori, 180.0, 0.0, 720.0);  //默认使用ZU710
    ini_file.get("TRAJ.TRAJ_MAX_ACC_ORI", modelData->maxTrajAcc_ori, 360.0, 0.0, 720.0);  //默认使用ZU710
    ini_file.get("TRAJ.TRAJ_MAX_VEL_ORI", modelData->maxTrajVel_ori, 180.0, 0.0, 720.0);  //默认使用ZU710

    ini_file.get("TRAJ.TRAJ_CLSN_ACC", modelData->clsnTrajAcc, 2000, 0, 20000);        //默认使用ZU20
    ini_file.get("TRAJ.TRAJ_CLSN_ACC_ORI", modelData->clsnTrajAcc_ori, 200, 0, 2000);  //默认使用ZU20

    // trajInifile.Find(&modelData->maxTrajVel, "TRAJ_MAX_VEL", "TRAJ");
    // trajInifile.Find(&modelData->maxTrajAcc, "TRAJ_MAX_ACC", "TRAJ");
    // trajInifile.Find(&modelData->maxTrajVel_ori, "TRAJ_MAX_VEL_ORI", "TRAJ");
    // trajInifile.Find(&modelData->maxTrajAcc_ori, "TRAJ_MAX_ACC_ORI", "TRAJ");
    ini_file.get("SAFEZONESPHERE.SAFEZONE_ELBOW_RADIUS", modelData->SafezoneElbowRadius, 151.59238107504, -360, 360.0);  //默认使用ZU710
    ini_file.get("SAFEZONESPHERE.SAFEZONE_ELBOW_OFFSET", modelData->SafezoneElbowOffset, -86.5, -360, 360.0);            //默认使用ZU710
    // trajInifile.Find(&modelData->SafezoneElbowRadius, "SAFEZONE_ELBOW_RADIUS", "SAFEZONESPHERE");
    // trajInifile.Find(&modelData->SafezoneElbowOffset, "SAFEZONE_ELBOW_OFFSET", "SAFEZONESPHERE");

    //momentum params
    ini_file.get("MOMENTUMLIMITDESIGNED.MIN_MOMENTUM", modelData->minMomentum, 0.1, 0.0, 5000.0);    //默认使用ZU710
    ini_file.get("MOMENTUMLIMITDESIGNED.MAX_MOMENTUM", modelData->maxMomentum, 100.0, 0.0, 5000.0);  //默认使用ZU710
    // trajInifile.Find(&modelData->minMomentum, "MIN_MOMENTUM", "MOMENTUMLIMITDESIGNED");
    // trajInifile.Find(&modelData->maxMomentum, "MAX_MOMENTUM", "MOMENTUMLIMITDESIGNED");

    //robotPower params
    ini_file.get("ROBOTPOWERLIMITDESIGNED.MIN_ROBOTPOWER", modelData->minRobotPower, 70, 0.0, 5000.0);      //默认使用ZU710
    ini_file.get("ROBOTPOWERLIMITDESIGNED.MAX_ROBOTPOWER", modelData->maxRobotPower, 2400.0, 0.0, 5000.0);  //默认使用ZU710
    // trajInifile.Find(&modelData->minRobotPower, "MIN_ROBOTPOWER", "ROBOTPOWERLIMITDESIGNED");
    // trajInifile.Find(&modelData->maxRobotPower, "MAX_ROBOTPOWER", "ROBOTPOWERLIMITDESIGNED");

    // safty params
    std::vector<double> v_robotPowerLimit(2, 0.0);
    std::vector<double> v_robotMomentumLimit(2, 0.0);
    std::vector<double> v_robotEndforceLimit(2, 0.0);
    ini_file.get("FUNCTIONSAFTYPARA.ROBOTPOWERLIMIT", v_robotPowerLimit, {100, 2000.0}, 0, 10000);        //默认使用ZU710
    ini_file.get("FUNCTIONSAFTYPARA.ROBOTMOMENTUMLIMIT", v_robotMomentumLimit, {5, 100.0}, 0, 10000);     //默认使用ZU710
    ini_file.get("FUNCTIONSAFTYPARA.ROBOTENDFORCELIMIT", v_robotEndforceLimit, {100.0, 250.0}, 0, 5000);  //默认使用ZU710
    for (int i = 0; i < 2; i++)
    {
        modelData->robotPowerLimit[i] = v_robotPowerLimit[i];
        modelData->robotMomentumLimit[i] = v_robotMomentumLimit[i];
        modelData->robotEndforceLimit[i] = v_robotEndforceLimit[i];
    }
    ini_file.get("FUNCTIONSAFTYPARA.ROBOTVELLIMIT", modelData->minRobotPower, 500.0, 0.0, 1000.0);  //默认使用ZU710
    // trajInifile.FindArray(modelData->robotPowerLimit, 2, "ROBOTPOWERLIMIT", "FUNCTIONSAFTYPARA");
    // trajInifile.FindArray(modelData->robotMomentumLimit, 2, "ROBOTMOMENTUMLIMIT", "FUNCTIONSAFTYPARA");
    // trajInifile.FindArray(modelData->robotEndforceLimit, 2, "ROBOTENDFORCELIMIT", "FUNCTIONSAFTYPARA");
    // trajInifile.Find(&modelData->robotVelocityLimit, "ROBOTVELLIMIT", "FUNCTIONSAFTYPARA");
}

static void readMotionLimit(Util::IniParser& ini_file, RobotModelData* modelData)
{
    ini_file.get("TIO.DI_NUM", modelData->minRobotPower, 2, 0.0, 1000.0);  //默认使用ZU725
    ini_file.get("TIO.DO_NUM", modelData->minRobotPower, 2, 0.0, 1000.0);  //默认使用ZU725
    ini_file.get("TIO.AI_NUM", modelData->minRobotPower, 1, 0.0, 1000.0);  //默认使用ZU725
    ini_file.get("TIO.AO_NUM", modelData->minRobotPower, 0, 0.0, 1000.0);  //默认使用ZU725
    // trajInifile.Find(&modelData->tioInfo.diNum, "DI_NUM", "TIO");
    // trajInifile.Find(&modelData->tioInfo.doNum, "DO_NUM", "TIO");
    // trajInifile.Find(&modelData->tioInfo.aiNum, "AI_NUM", "TIO");
    // trajInifile.Find(&modelData->tioInfo.aoNum, "AO_NUM", "TIO");
}

static void readDynIdentifyTraj(Util::IniParser& ini_file, long serialNum, RobotModelData* modelData)
{
    // std::vector<double> val(ELE_IDENTI_TRAJ_DIMENSION, 0.0);
    // std::vector<double> def_val(ELE_IDENTI_TRAJ_DIMENSION, 0.0);

    // ini_file.get("DYNAMICS.TRAJ_PARA", val, def_val, -100, 100);

    // for (int i = 0; i < ELE_IDENTI_TRAJ_DIMENSION; i++) { modelData->dynamics_identify_traj_para[i] = val[i]; }
}

int iniLoadRobotModelData(int robotModel, long serialNum, RobotModelData* modelData)
{
    int i = 0;

    if (modelData == nullptr)
        return -1;

    std::string modelIniPath = deterRobotModel(robotModel);

    // read the model data from the ini file
    Util::IniParser model_ini_file(modelIniPath.c_str());

    try
    {
        readMotionLimit(model_ini_file, serialNum, modelData);
        readMotionLimit(model_ini_file, modelData);
        readDynIdentifyTraj(model_ini_file, serialNum, modelData);
        modelData->model = robotModel;
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniLoadHistoryDHParam(Util::IniParser* ini_file, int serialNum, DHParam* dhParam)
{
    if (dhParam == nullptr)
        return -1;

    try
    {
        char strSNKey[64];
        sprintf(strSNKey, "DHPARAM.%09d", serialNum);
        for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++)
        {
            dhParam->alpha[i] = 0.0;
            dhParam->a[i] = 0.0;
            dhParam->d[i] = 0.0;
            dhParam->joint_homeoff[i] = 0.0;
        }

        float alpha_zu[6] = {0, 90, 0, 0, 90, -90};
        float alpha_mc[6] = {0, 90, 0, -90, 90, -90};
        if (isZuCProCobot(serialNum) || isMaxCobot(serialNum))
        {
            std::vector<double> read_res(24, 0.0);
            std::vector<double> default_fDHParam = {114.75, -116.25, 103.25, 93.75, 353, 303, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
            // if (trajInifile->FindArray(fDHParam, 24, strSNKey, "DHPARAM") == IniFile::ERR_NONE)
            if (ini_file->get(strSNKey, read_res, default_fDHParam, -2000.0, 2000.0) == Util::IniParser::ini_errcode::OK)
            {
                dhParam->d[0] = read_res[0];
                dhParam->d[3] = read_res[1];
                dhParam->d[4] = read_res[2];
                dhParam->d[5] = read_res[3];
                dhParam->a[2] = read_res[4];
                dhParam->a[3] = read_res[5];
                for (int i = 0; i < 6; i++)
                {
                    dhParam->joint_homeoff[i] = read_res[6 + i];
                    if (read_res[12 + i] == 0)  //对于部分机型可能没有alpha参数，此时读到的fDHParam为其初始值0，此时采用默认的alpha
                        dhParam->alpha[i] = alpha_zu[i];
                    else
                        dhParam->alpha[i] = read_res[12 + i];
                }
                _PRINT("History DH parameters: %f, %f, %f\n", read_res[0], read_res[1], read_res[2]);
            }
            else
            {
                _PRINT("History DH parameters: read failed\n");
                return -1;
            }
        }
        else if (isMiniCobot(serialNum))
        {  // theta is incr value
            std::vector<double> read_res(24, 0.0);
            std::vector<double> default_fDHParam = {187, 6, 0, 210.5, 0, 159.3, 0, 0, 210, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 90, 0, -90, 90, -90};
            //if (trajInifile->FindArray(fDHParam, 24, strSNKey, "DHPARAM") == IniFile::ERR_NONE)
            if (ini_file->get(strSNKey, read_res, default_fDHParam, -2000.0, 2000.0) == Util::IniParser::ini_errcode::OK)
            {
                for (int i = 0; i < 6; i++)
                {
                    dhParam->d[i] = read_res[i];
                    dhParam->a[i] = read_res[i + 6];
                    dhParam->joint_homeoff[i] = read_res[i + 12];
                    if (read_res[18 + i] == 0)  //对于部分机型可能没有alpha参数，此时读到的fDHParam为其初始值0，此时采用默认的alpha
                        dhParam->alpha[i] = alpha_mc[i];
                    else
                        dhParam->alpha[i] = read_res[18 + i];
                }
                _PRINT("Cobot History DH parameters: %f, %f, %f\n", read_res[0], read_res[1], read_res[2]);
            }
            else
            {
                return -1;
            }
        }
        else if (is7JointCobot(serialNum))
        {
            std::vector<double> read_res(28, 0.0);
            std::vector<double> default_fDHParam = {185.0, 0.0, 330.0, 0.0, 220.0, 0.0, 140.0, 0,   0,  0,   0,  0,   0,  0,
                                                    0,     0,   0,     0,   0,     0,   0,     -90, 90, -90, 90, -90, 90, 0};
            //if (trajInifile->FindArray(fDHParam, 24, strSNKey, "DHPARAM") == IniFile::ERR_NONE)
            if (ini_file->get(strSNKey, read_res, default_fDHParam, -2000.0, 2000.0) == Util::IniParser::ini_errcode::OK)
            {
                for (int i = 0; i < 7; i++)
                {
                    dhParam->d[i] = read_res[i];
                    dhParam->a[i] = read_res[i + 7];
                    dhParam->joint_homeoff[i] = read_res[i + 14];
                    dhParam->alpha[i] = read_res[21 + i];
                }
                _PRINT("Cobot History DH parameters: %f, %f, %f\n", read_res[0], read_res[1], read_res[2]);
            }
            else
            {
                return -1;
            }
        }
        else
        {
            return -1;
        }
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}
int iniEleIdentifyTraj(int robotModel, long serialNum, double* traj_para)
{
    // std::string modelIniPath = deterRobotModel(robotModel);
    // Util::IniParser ini_file(modelIniPath.c_str());
    // std::vector<double> param;
    // std::vector<double> default_val;
    // for (int i = 0; i < ELE_IDENTI_TRAJ_DIMENSION; i++) { default_val.push_back(0.0); }
    // if (ini_file.get("DYNAMICS.TRAJ_PARA", param, default_val, -100, 100))
    // {
    //     return -1;
    // }
    // for (int i = 0; i < ELE_IDENTI_TRAJ_DIMENSION; i++) { traj_para[i] = param[i]; }
    return 0;
}

int isZuCProCobot(uint32_t serialNum)
{
    if (serialNum / 100000000 == 0)
        return 1;
    return 0;
}
int isMiniCobot(uint32_t serialNum)
{
    if (serialNum / 100000000 == 1)
        return 1;
    return 0;
}

int is7JointCobot(uint32_t serialNum)
{
    if (serialNum / 100000000 == 2)
        return 1;
    return 0;
}

int isMaxCobot(uint32_t serialNum)
{
    if (serialNum / 100000000 == 3)
        return 1;
    return 0;
}